#include <ros/ros.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <signal.h>
#include <termios.h>
#include <linux/input.h>
#include <serial/serial.h>
#include <tf/tf.h>
#include <boost/thread.hpp>
#include <tecbot_msgs/nav_to_state.h>
#include <std_msgs/Int8.h>
#include <tecbot_msgs/battery_state.h>
#include <common/common_state_code.h>
#include <navigation/navigation_state_code.h>
#include <localization/localization_sources.h>

#define NODE_NAME "screen_handler"
#define TAG "屏幕驱动"

using namespace std;

// 定义参数
// 控制频率
float control_freq = 5;

// 连接参数
string port_name;
int baudrate;

// 包尾
uint8_t comm_tail = 0xff;

// 定义串口
serial::Serial serial_port;

// 定义通讯参数
string packet_send_buffer;

// 定义电池电量数据
tecbot_msgs::battery_state battery_data;

// 定义导航状态数据
tecbot_msgs::nav_to_state nav_to_state_data;

// 灯光数据
std_msgs::Int8 light_data;
int last_light_color = -1;

// 定义接收器
ros::Subscriber battery_data_sub;
ros::Subscriber nav_to_state_sub;

// 定义发布器
ros::Publisher light_pub;

// 功能函数
// 初始化函数
bool serial_init()
{
    bool ret = false;

    try
    {
        // 设置端口名称
        serial_port.setPort(port_name);

        // 设置端口波特率
        serial_port.setBaudrate(baudrate);

        // 设置端口超时
        serial::Timeout timeout = serial::Timeout::simpleTimeout(2000);
        serial_port.setTimeout(timeout);

        // 开启串口
        serial_port.open();

        ret = true;
    }
    catch (...)
    {
        ROS_ERROR("%s：屏幕串口连接失败。", TAG);
    }

    return ret;
}

// 设置灯带颜色
void set_light_color(int color)
{
    if (color != last_light_color)
    {
        light_data.data = color;
        light_pub.publish(light_data);

        last_light_color = color;
    }
}

// 发送数据
void send_data(string content)
{
    try
    {
        serial_port.write(content);
        serial_port.write((const uint8_t *)(&comm_tail), 1);
        serial_port.write((const uint8_t *)(&comm_tail), 1);
        serial_port.write((const uint8_t *)(&comm_tail), 1);
    }
    catch (...)
    {
        ROS_ERROR("%s：串口连接失败。", TAG);

        // 尝试重连
        serial_init();
    }
}

// 回调函数
// 电池电量回调
void battery_data_callback(const tecbot_msgs::battery_state::ConstPtr &msg)
{
    battery_data = *msg;
}

// 导航状态回调
void nav_to_state_callback(const tecbot_msgs::nav_to_state::ConstPtr &msg)
{
    nav_to_state_data = *msg;
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, NODE_NAME);

    // 设置控制台本地化，以显示中文
    setlocale(LC_CTYPE, "zh_CN.utf8");

    // 打印调试信息
    ROS_INFO("%s:节点已启动。", TAG);

    // 定义ROS节点处理程序
    ros::NodeHandle node_handle;

    // 定义ROS参数获取程序
    ros::NodeHandle node_handle_param("~/screen");

    // 获取参数
    node_handle_param.param<float>("control_freq", control_freq, 5);

    node_handle_param.param<string>("port_name", port_name, "/dev/screen_con");
    node_handle_param.param<int>("baudrate", baudrate, 9600);

    // 初始化系统
    if (serial_init())
    {
        // 初始化订阅器
        battery_data_sub = node_handle.subscribe<tecbot_msgs::battery_state>("tecbot/hardware/bms", 10, battery_data_callback);
        nav_to_state_sub = node_handle.subscribe<tecbot_msgs::nav_to_state>("tecbot/navigation/state", 10, nav_to_state_callback);

        // 初始化发布器
        light_pub = node_handle.advertise<std_msgs::Int8>("tecbot/control/light", 1, true);

        // 定义循环速度控制
        auto rate = ros::Rate(control_freq);

        // 等待系统初始化
        ros::Duration(20).sleep();

        while (node_handle.ok())
        {
            // 处理回调函数
            ros::spinOnce();

            // 发送心跳包
            send_data("heartbeat.val=1");

            // 电池电量
            send_data("percent.val=" + to_string((int)battery_data.soc));

            // 状态码
            if (battery_data.soc > 0 && battery_data.soc < 10)
            {
                send_data("state.val=" + to_string(4));

                set_light_color(NAV_LIGHT_ERROR);
            }
            else if (nav_to_state_data.state == NAV_STATE_RUNNING ||
                     nav_to_state_data.state == NAV_STATE_RECOVERY)
            {
                send_data("state.val=" + to_string(2));
            }
            else if (nav_to_state_data.state == NAV_STATE_PAUSE)
            {
                send_data("state.val=" + to_string(1));
            }
            else if (nav_to_state_data.state == NAV_STATE_REACHED)
            {
                send_data("state.val=" + to_string(0));
            }
            else if (nav_to_state_data.state == NAV_STATE_ERROR)
            {
                send_data("state.val=" + to_string(5));
            }
            else if (nav_to_state_data.state == NAV_STATE_BLOCKED)
            {
                send_data("state.val=" + to_string(3));
            }
            else
            {
                send_data("state.val=" + to_string(0));
            }

            // 循环频率控制
            rate.sleep();
        }
    }

    return 0;
}